struct AlgConnAStar : public AlgConn {
	static AlgConn* creater() {
		return new AlgConnAStar{};
	}
	void setParams(const std::vector<ParamValue>& params) override {
		v0=params[0].getFloat();
		maxdist = params[1].getFloat();
		maxarea = params[2].getFloat();
		//length=params[1].getFloat();
	}
	std::vector<ParamValue> getParams() const override {
		std::vector<ParamValue> vl;
		vl.push_back(ParamValue{ v0 });
		vl.push_back(ParamValue{ maxdist });
		vl.push_back(ParamValue{ maxarea });
		//vl.push_back(ParamValue{length});
		return vl;
	}
	AlgConnAStar() : AlgConn{}, v0{.007} { }

	struct AstarNode {
		int64_t came_from, came_from2;
		int64_t index;
		double g_score;
		double h_score;
		int8_t state; // -+3, l/r open; -+2, l/r closed; -+1, l/r collide, 0 skip
	};

	void emptyResult(Path* pth) {
		pth->points.clear();
		pq.clear();
		store.clear();
		idx2node.clear();
	}
	// improvements?
	double heuristic_cost_estimate(int64_t a, int64_t b, int64_t c, double g) {
		double ax, ay, az, bx, by, bz, cx, cy, cz;
		index2xyzf<0>(a, &ax, &ay, &az);
		index2xyzf<0>(b, &bx, &by, &bz);
		index2xyzf<0>(c, &cx, &cy, &cz);
		ax-=bx;
		ay-=by;
		az-=bz;
		cx-=bx;
		cy-=by;
		cz-=bz;

		auto ab=std::sqrt(ax*ax+ay*ay+az*az);
		auto bc=std::sqrt(cx*cx+cy*cy+cz*cz);
		return bc/(ab/g+v0);
	}
	double dist_between(int64_t a, int64_t b, double d) {
		auto aa=xfunc<0>(cube<0>(a))*255;
		auto bb=xfunc<0>(cube<0>(b))*255;
		return (1/(aa+v0)+1/(bb+v0))*d/2;
	}

	void estimateRadius(std::vector<Point>& pth) {
		double res=xres<0>();
		double maxr=65536/256;
		if(res>yres<0>()) res=yres<0>();
		if(res>zres<0>()) res=zres<0>();
		res/=2;
		for(size_t i=0; i<pth.size(); i++) {
			std::vector<double> rvals{};
			int64_t idx_center=xyzf2index<0>(pth[i].x(), pth[i].y(), pth[i].z());
			double val_center=xfunc<0>(cube<0>(idx_center))*255;
			for(int j=0; j<500; j++) {
				int l=rand();
				double dx=l&0xFF, dy=(l>>8)&0xFF, dz=(l>>8)&0xFF;
				double len=sqrt(dx*dx+dy*dy+dz*dz);
				dx/=len;
				dy/=len;
				dz/=len;
				int mdiff=512;
				double rval=0;
				int val_prev=val_center;
				for(double len=res; len<=maxr; len+=res) {
					int64_t idx=xyzf2index<0>(pth[i].x()+dx*len, pth[i].y()+dy*len, pth[i].z()+dz*len);
					if(idx<0)
						break;
					double val=xfunc<0>(cube<0>(idx))*255;
					auto diff=val-val_prev;
					if(diff<mdiff) {
						mdiff=diff;
						rval=len;
					}
					val_prev=val;
					if(i==0) {
						//fprintf(stderr, "%lf, ", diff);
					}
					if(val<-mdiff)
						break;
				}
					if(i==0) {
						//fprintf(stderr, "%lf\n", rval);
					}
				rvals.push_back(rval);
			}
			std::sort(rvals.begin(), rvals.end());
			double sum=0;
			for(int j=100; j<200; j++) {
				sum+=rvals[j];
			}
			pth[i].r(sum/100);
					//fprintf(stderr, "asdf %lf\n", pth[i].r());
		}
	}

	double v0;
	double maxdist;
	double maxarea;
	//bool success;
	std::vector<size_t> pq;
	std::vector<AstarNode> store;
	std::unordered_map<int64_t, size_t> idx2node;

	public:
	void connect(const Position& p0, const Position& p1, Graph gr, Path* pth) override {
		auto AstarNodeCmp=[this](size_t a, size_t b) -> bool {
			return store[a].g_score+store[a].h_score>store[b].g_score+store[b].h_score;
		};

		bool success=false;
		int64_t start=xyzf2index<0>(p0.point.x(), p0.point.y(), p0.point.z());
		int64_t goal=xyzf2index<0>(p1.point.x(), p1.point.y(), p1.point.z());
		if(start==goal)
			return emptyResult(pth);

		size_t nn;
		{
			store.emplace_back();
			nn=store.size()-1;
			store[nn].came_from=-1;
			store[nn].came_from2=-1;
			store[nn].index=start;
			store[nn].g_score=0;
			store[nn].h_score=0;
			store[nn].state=-3;
			idx2node[start]=nn;
			pq.push_back(nn); std::push_heap(pq.begin(), pq.end(), AstarNodeCmp);
		}
		{
			store.emplace_back();
			nn=store.size()-1;
			store[nn].came_from=-1;
			store[nn].came_from2=-1;
			store[nn].index=goal;
			store[nn].g_score=0;
			store[nn].h_score=0;
			store[nn].state=3;
			idx2node[goal]=nn;
			pq.push_back(nn); std::push_heap(pq.begin(), pq.end(), AstarNodeCmp);
		}

		struct { int32_t dx, dy, dz; double dist; } all_neighbors [] = {
			{-1, 0, 0, xres<0>()},
			{1, 0, 0, xres<0>()},
			{0, -1, 0, yres<0>()},
			{0, 1, 0, yres<0>()},
			{0, 0, -1, zres<0>()},
			{0, 0, 1, zres<0>()}
		};

		size_t currentp;
		while(!pq.empty()) {
			currentp=pq.front();
			std::pop_heap(pq.begin(), pq.end(), AstarNodeCmp); pq.pop_back();
			auto st=store[currentp].state;
			bool skip=false;
			switch(st) {
				case 0: skip=true; break;
				case -1: case 1: success=true; break;
				default:
					fprintf(stderr, "WRONG state1 %hhd!\n", st);
					return emptyResult(pth);
				case -3: st++; break;
				case 3: st--; break;
			}
			if(skip)
				continue;
			if(success)
				break;
			if(!sync(100)) {
				fprintf(stderr, "abort\n");
				break;
			}
			store[currentp].state=st;
			auto current=store[currentp].index;

			int32_t x, y, z;
			index2xyzi<0>(current, &x, &y, &z);
			for(auto& delta: all_neighbors) {
				int32_t neighbor_x=x+delta.dx;
				int32_t neighbor_y=y+delta.dy;
				int32_t neighbor_z=z+delta.dz;
				if(neighbor_x<0 || neighbor_x>=width<0>())
					continue;
				if(neighbor_y<0 || neighbor_y>=height<0>())
					continue;
				if(neighbor_z<0 || neighbor_z>=depth<0>())
					continue;
				int64_t neighbor=xyzi2index<0>(neighbor_x, neighbor_y, neighbor_z);
				auto neighbori=idx2node.find(neighbor);
				bool inserted=false;
				if(neighbori!=idx2node.end()) {
					auto neighborn=neighbori->second;
					bool cont=false;
					bool upd1=false;
					auto st2=store[neighborn].state;
					switch(st2) {
						default:
							fprintf(stderr, "WRONG state2 %hhd\n", st2);
							return emptyResult(pth);
						case -2: case +2:
							cont=true;
							break;
						case -3:
						case 3:
							break;
						case -1:
						case 1:
							upd1=true;
							break;
					}
					if(cont)
						continue;
					double tentative_g_score=store[currentp].g_score+dist_between(current, neighbor, delta.dist);
					if(upd1) {
						auto score_prev=st*st2<0?store[neighborn].h_score:store[neighborn].g_score;
						if(tentative_g_score<score_prev) {
							store[neighborn].state=0;
							store.emplace_back();
							nn=store.size()-1;
							store[nn].came_from=current;
							store[nn].came_from2=st*st2<0?store[neighborn].came_from:store[neighborn].came_from2;
							store[nn].g_score=tentative_g_score;
							store[nn].h_score=st*st2<0?store[neighborn].g_score:store[neighborn].h_score;
							store[nn].state=st<0?-1:1;
							inserted=true;
						}
					} else {
						if(st*st2<0) {
							store[neighborn].state=0;
							store.emplace_back();
							nn=store.size()-1;
							store[nn].came_from=current;
							store[nn].came_from2=store[neighborn].came_from;
							store[nn].g_score=tentative_g_score;
							store[nn].h_score=store[neighborn].g_score;
							store[nn].state=st<0?-1:1;
							inserted=true;
						} else if(tentative_g_score<store[neighborn].g_score) {
							store[neighborn].state=0;
							store.emplace_back();
							nn=store.size()-1;
							store[nn].came_from=current;
							store[nn].came_from2=store[neighborn].came_from2;
							store[nn].g_score=tentative_g_score;
							store[nn].h_score=heuristic_cost_estimate(st<0?start:goal, neighbor, st<0?goal:start, tentative_g_score);
							store[nn].state=st2;
							inserted=true;
						}
					}
				} else {
					double tentative_g_score=store[currentp].g_score+dist_between(current, neighbor, delta.dist);
					store.emplace_back();
					nn=store.size()-1;
					store[nn].came_from=current;
					store[nn].came_from2=-1;
					store[nn].g_score=tentative_g_score;
					store[nn].h_score=heuristic_cost_estimate(st<0?start:goal, neighbor, st<0?goal:start, tentative_g_score);
					store[nn].state=st<0?-3:3;
					inserted=true;
				}
				if(inserted) {
					//store[nn].came_from=current;
					store[nn].index=neighbor;
					idx2node[neighbor]=nn;
					pq.push_back(nn); std::push_heap(pq.begin(), pq.end(), AstarNodeCmp);
				}
			}
		}

		if(!success)
			return emptyResult(pth);

		std::vector<int64_t> idxes0;
		std::vector<int64_t> idxes1;
		idxes0.push_back(store[currentp].index);
		{
			auto idx=store[currentp].came_from;
			while(idx!=-1) {
				idxes0.push_back(idx);
				idx=store[idx2node[idx]].came_from;
			}
		}
		{
			auto idx=store[currentp].came_from2;
			while(idx!=-1) {
				idxes1.push_back(idx);
				idx=store[idx2node[idx]].came_from;
			}
		}
		pth->edge0=p0.edge;
		pth->index0=p0.index;
		pth->edge1=p1.edge;
		pth->index1=p1.index;
		pth->points.clear();

		std::vector<int64_t> idxes;
		if(store[currentp].state>0)
			std::swap(idxes0, idxes1);
		while(!idxes0.empty()) {
			idxes.push_back(idxes0.back());
			idxes0.pop_back();
		}
		for(auto i: idxes1)
			idxes.push_back(i);
		if(idxes.size()<2)
			return emptyResult(pth);

		for(auto v: idxes) {
			double mx, my, mz;
			index2xyzf<0>(v, &mx, &my, &mz);
			pth->points.push_back({mx, my, mz});
		}
		/////


					////////// /// /////
//heap:
//
	//////////////
	///////



		if(p0.edge) {
			pth->points[0]=p0.edge.points()[p0.index];
		}
		if(p1.edge) {
			pth->points.back()=p1.edge.points()[p1.index];
		}
		//estimateRadius(pth->points);
		float length = p0.point.distTo(p1.point);
		float ratio = length / 100.0;
		ratio = ratio < 1 ? 1 : ratio;
		pth->points=reducePointsVisvalingam(pth->points, maxdist*ratio, maxarea*ratio);
		//estimateRadius(pth->points);
		pq.clear();
		store.clear();
		idx2node.clear();
	}
	std::vector<int32_t> getUpdate() const override {
		std::vector<int32_t> data{};
		for(auto n: pq) {
			if(store[n].state==0)
				continue;
			double x, y, z;
			index2xyzf<0>(store[n].index, &x, &y, &z);
			Point p(x, y, z);
			data.push_back(p._x);
			data.push_back(p._y);
			data.push_back(p._z);
		}
		return data;
	}
};
